/*******************************************************************************
* This is the main program for the EasyPilot.
*
* Author: Kong Wai Weng
* Email:  waiweng83@gmail.com
*******************************************************************************/



#include <stdio.h>
#include <string.h>
#include "system.h"
#include "variables.h"

#include "FreeRTOS.h"
#include "task.h"

#include "hal_timer.h"
#include "hal_adc.h"
#include "hal_uart.h"
#include "hal_i2c.h"
#include "hal_spi.h"
#include "hal_oc.h"
#include "hal_ic.h"
#include "hal_sensors.h"
#include "hal_bmp085.h"
#include "hal_serialflash.h"
#include "hal_gps.h"

#include "dcm.h"
#include "airspeed.h"
#include "control.h"
#include "error.h"
#include "ep_mavlink.h"
#include "slcd.h"
#include "datalog.h"
#include "cml.h"
#include "navigation.h"



/*******************************************************************************
* DEVICE CONFIGURATION WORDS                                                   *
*******************************************************************************/

// No Boot Segment Data RAM.
// No Boot Segment Program Memory.
// Boot Segment Write Protect Off.
_FBS(RBS_NO_BOOT_RAM & BSS_NO_BOOT_CODE & BWRP_WRPROTECT_OFF)

// No Secure Segment Data RAM.
// No Secure Segment Program Memory.
// Secure Segment Write Protect Off.
_FSS(RSS_NO_SEC_RAM & SSS_NO_SEC_CODE & SWRP_WRPROTECT_OFF)

// Code Protect Off.
// Code Protect Off.
// Write Protect Off.
_FGS(GSS_OFF & GCP_OFF & GWRP_OFF)

// Fast RC oscillator.
// Two-speed Oscillator Startup Off.
_FOSCSEL(FNOSC_FRC & IESO_OFF)

// Only clock switching enabled.
// OSC2 is clock O/P.
// XT Oscillator.
_FOSC(FCKSM_CSECMD & OSCIOFNC_OFF & POSCMD_XT)

// Watchdog Timer Off.
// Windowed WDT Off.
// Clock switch to PLL source will wait until the PLL lock signal is valid.
// Watchdog Prescaler 1:32.
// Watchdog Postscaler 1:1.
_FWDT(FWDTEN_OFF & WINDIS_OFF & PLLKEN_ON & WDTPRE_PR32 & WDTPOST_PS1)

// Power-on Reset Value - 128ms.
_FPOR(FPWRT_PWR128)

// JTAG Off.
// ICD communicates on PGC1/EMUC1 and PGD1/EMUD1.
_FICD(JTAGEN_OFF & ICS_PGD1)



/*******************************************************************************
* PRIVATE CONSTANT DEFINITION                                                  *
*******************************************************************************/





/*******************************************************************************
* PRIVATE FUNCTION PROTOTYPES                                                  *
*******************************************************************************/

static void prv_vInitializeSystem(void);
static unsigned int prv_uiCreateTasks(void);



/*******************************************************************************
* PRIVATE GLOBAL VARIABLES                                                     *
*******************************************************************************/

//unsigned char ucWriteBuffer[5120] = {0};
//unsigned char ucReadBuffer[5120] = {0};



/*******************************************************************************
* MAIN FUNCTION                                                                *
*******************************************************************************/
int main(void)
{
	// Initialize the system.
	prv_vInitializeSystem();

    // Blink the LED once.
    LED1 = 1;
    LED2 = 1;
    LED3 = 1;
    __delay_ms(500);
    LED1 = 0;
    LED2 = 0;
    LED3 = 0;

	
    // Wait to arm the UAV.
    // Also make sure the RC pulse is valid.
    while ((uiGetRcRx(AILERON_CHANNEL) < PULSE_WIDTH_1750) && (xSystemError.bRcRxError == 0));



    // Create tasks.
    if (prv_uiCreateTasks() != pdPASS) {
        unsigned char szMsgCreateTaskFail[] = "Create Task Fail\r\n";
        ucSendToPc(szMsgCreateTaskFail, sizeof(szMsgCreateTaskFail));

        // Blink the LED.
        LED1 = 0;
        LED2 = 0;
        LED3 = 0;
        while (1) {
            LED1 ^= 1;
            __delay_ms(50);
        }
    }



    // Start the scheduler.
    vTaskStartScheduler();

    // We should not reach here.
    // Display error message, if we ever come to here.
    unsigned char szMsgInsufficientRam[] = "Insufficient RAM\r\n";
    ucSendToPc(szMsgInsufficientRam, sizeof(szMsgInsufficientRam));

    // Blink the LED.
    LED1 = 0;
    LED2 = 0;
    LED3 = 0;

    while (1) {
        LED1 ^= 1;
        __delay_ms(50);
    }



    return 0;
}



/*******************************************************************************
* PRIVATE FUNCTION: prv_vInitializeSystem
*
* PARAMETERS:
* ~ void
*
* RETURN:
* ~ void
*
* DESCRIPTIONS:
* Setup the peripherals to be used in this system.
*
*******************************************************************************/
static void prv_vInitializeSystem(void)
{
	// Configure PLL prescaler, PLL postscaler, PLL divisor so that FOSC = 80MHz.
	// FOSC = FIN * M / (N1 * N2).
	// FCY = FOSC / 2.
	CLKDIVbits.PLLPRE = 0;		// N1 = 2.
	CLKDIVbits.PLLPOST = 0;		// N2 = 2.
	PLLFBDbits.PLLDIV = 78;		// M = 80.
	
	// Initiate Clock Switch to Primary Oscillator with PLL (NOSC = 0b011).
	__builtin_write_OSCCONH(0x03);
	__builtin_write_OSCCONL(0x01);
	
	// Wait for Clock switch to occur.
	while (OSCCONbits.COSC != 0b011);
	
	// Wait for PLL to lock.
	while(OSCCONbits.LOCK == 0);
	
	
	
	// Initialize the variables.
	vInitVar();
	
	// Reset the outputs.
	LATA = 0;
	LATB = 0;
	LATC = 0;
	LATD = 0;
	LATE = 0;
	LATF = 0;
	LATG = 0;
	
	// Set the SPI Chip Select pin.
	SFLASH_CSN = 1;
	
	// Initialize the IOs direction.
	TRISA = 0b0000000000011100;
	TRISB = 0b0000111011000011;
	TRISC = 0b1001000000000000;
	TRISD = 0b1111111100000000;
	TRISE = 0b0000000000001000;
	TRISF = 0b0000000010010100;
	TRISG = 0b0000000010001100;
	
	
	
	// Initialize UART 1 (GPS).
	vInitUart1();
	
	// Initialize UART 2 (PC / Telemetry).
	vInitUart2();
	
	// Initialize I2C 1 (Onboard Sensors).
	vInitI2c1();
	
	// Initialize SPI 1 (Serial Flash).
	vInitSpi1();
	
	// Initialize Output Compare and Timer 2.
	vInitOc();
	vInitTimer2();
	
	// Initialize Input Capture and Timer 3.
	vInitIc();
	vInitTimer3();
	
	// Initialize ADC.
	vInitAdc();
	
	
	
	// Delay for a while so that the external sensors and modules are powered up.
	__delay_ms(200);
	
	
	
	// Initialize sensors.
	vInitSensors();
	vInitBmp085();

        // Calibrate the current offset.
        vCalibrateCurrentOffset();
	
	
	
	// Unprotect the serial flash.
	vSFlashGlobalUnprotect();
}	



/*******************************************************************************
* PRIVATE FUNCTION: prv_uiCreateTasks
*
* PARAMETERS:
* ~ void
*
* RETURN:
* ~ Return pdPASS if success.
* ~ Return pdFAIL if fail.
*
* DESCRIPTIONS:
* Create tasks for the system.
*
*******************************************************************************/
static unsigned int prv_uiCreateTasks(void)
{
	// Dummy task handle.
	xTaskHandle xDummyTaskHandle;
	
	
	
	// Create task for the DCM loop.
	if (xTaskCreate(taskDcmLoop, (signed char*)"DCM", 1024, NULL, 5, &xDummyTaskHandle) != pdPASS) {
		return pdFAIL;
	}
	
	// Create task for the PID control loop.
	if (xTaskCreate(taskControlLoop, (signed char*)"PID", 1024, NULL, 5, &xDummyTaskHandle) != pdPASS) {
		return pdFAIL;
	}
	
	
	
	// Create task for the navigation loop.
	if (xTaskCreate(taskNavigationLoop, (signed char*)"NAV", 1024, NULL, 4, &xDummyTaskHandle) != pdPASS) {
		return pdFAIL;
	}
	
	// Create task for the BMP085 loop.
	if (xTaskCreate(taskBmp085Loop, (signed char*)"BMP085", configMINIMAL_STACK_SIZE, NULL, 4, &xDummyTaskHandle) != pdPASS) {
		return pdFAIL;
	}

	// Create task for the airspeed loop.
	if (xTaskCreate(taskAirspeedLoop, (signed char*)"AIRSPD", configMINIMAL_STACK_SIZE, NULL, 4, &xDummyTaskHandle) != pdPASS) {
		return pdFAIL;
	}
	
	
	
//	// Create task for data logging
//	if (xTaskCreate(taskDataLog, (signed char*)"LOG", 1024, NULL, 2, &xDummyTaskHandle) != pdPASS) {
//		return pdFAIL;
//	}
	
	
	
	// Create task for LED Blinking base on error flag.
	if (xTaskCreate(taskLedError, (signed char*)"LED", configMINIMAL_STACK_SIZE, NULL, 1, &xDummyTaskHandle) != pdPASS) {
		return pdFAIL;
	}
	
	// Create task for the RC Rx watchdog.
	if (xTaskCreate(taskRcRxWatchdog, (signed char*)"RCWD", configMINIMAL_STACK_SIZE, NULL, 1, &xDummyTaskHandle) != pdPASS) {
		return pdFAIL;
	}
	
	// Create task for the GPS watchdog.
	if (xTaskCreate(taskGPSWatchdog, (signed char*)"GPSWD", configMINIMAL_STACK_SIZE, NULL, 1, &xDummyTaskHandle) != pdPASS) {
		return pdFAIL;
	}
	
	// Create task for sending MAVLink heartbeat.
	if (xTaskCreate(taskMavlinkHeartbeat, (signed char*)"MLHB", configMINIMAL_STACK_SIZE, NULL, 1, &xDummyTaskHandle) != pdPASS) {
		return pdFAIL;
	}
	
//	// Create task for displaying data in serial lcd.
//	if (xTaskCreate(taskSLcd, (signed char*)"SLCD", 1024, NULL, 1, &xDummyTaskHandle) != pdPASS) {
//		return pdFAIL;
//	}
	
//	// Create task for command line interface
//	if (xTaskCreate(taskCml, (signed char*)"CML", 1024, NULL, 1, &xDummyTaskHandle) != pdPASS) {
//		return pdFAIL;
//	}
	
	
	
	return pdPASS;
}	
